//
// Created by lqk on 2022/2/5.
//

#ifndef QUADCEREBELLUM_HIGHFREQUENCYTASKSWRAPPER_H
#define QUADCEREBELLUM_HIGHFREQUENCYTASKSWRAPPER_H
#include "PeriodicTask.h"
#include "LegsController.h"
#include "ConvexMPController.h"
#include "OrientationEstimator.h"
#include "PositionVelocityEstimator.h"
#include "StateEstimator.h"


class HighFrequencyTasksWrapper :public PeriodicTask{
public:
    HighFrequencyTasksWrapper(PeriodicTaskManager* taskManager,const float& period,const std::string& name,const QuadrupedSpecification& quadSpec): PeriodicTask(taskManager,period,name),m_quadKinematics(quadSpec),m_legsController(m_quadKinematics){}
    virtual void Init() override;
    virtual void RunOnce() override;
    virtual void PrintStatus() override;
private:
    virtual void CleanUp() override;
    QuadrupedKinematics m_quadKinematics;
    LegsController m_legsController;
    LegCommand m_legCommand[4];
    LegState m_legState[4];
    EstimatedState m_estimatedState;
    StateEstimator m_stateEstimator{&m_estimatedState};
    ConvexMPController m_convexMPC{0.002,15};
};


#endif //QUADCEREBELLUM_HIGHFREQUENCYTASKSWRAPPER_H
